%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Yifan Zhang
% Bejing University of Technology
% Copy Right 2023
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Naive Kalman Filter
% 简单的卡尔曼滤波
%
% 尝试使用一个简单的卡尔曼滤波，实现将两种算法得到的值融合
%
% INPUT: 
% x: 初始状态向量，2x1矩阵，包含两个整数
% y: 测量值，1x1矩阵，包含一个浮点数
%
% OUTPUT
%   xhat: 卡尔曼滤波后的角度值
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

function xhat = kalmanFilter(x, y)
% x: 初始状态向量，2x1矩阵，包含两个整数
% y: 测量值，1x1矩阵，包含一个浮点数
% 返回值：xhat，1x1矩阵，包含一个浮点数，是卡尔曼滤波的估计结果

% 系统模型和卡尔曼滤波参数
A = [1 1; 0 1]; % 状态转移矩阵
C = [1 0]; % 测量矩阵
Q = [0.01 0; 0 0.01]; % 状态噪声协方差矩阵
R = 0.1; % 测量噪声方差

% 初始化状态向量和状态估计协方差矩阵
xhat = [x(1); 0]; % 初始状态向量
P = eye(2); % 初始状态估计协方差矩阵

% 卡尔曼滤波过程
for k = 1:length(y)
    % 预测下一个状态向量
    xhatminus = A * xhat;
    % 预测下一个状态估计协方差矩阵
    Pminus = A * P * A' + Q;
    % 计算卡尔曼增益
    K = Pminus * C' / (C * Pminus * C' + R);
    % 更新状态向量
    xhat = xhatminus + K * (y(k) - C * xhatminus);
    % 更新状态估计协方差矩阵
    P = (eye(2) - K * C) * Pminus;
end

% 返回估计值
xhat = xhat(1);
end